#include <SoftwareSerial.h>
#include <Servo.h>
SoftwareSerial softSerial(10, 11); // RX, TX
Servo servo1;

int servopin=9;//定义数字接口 9 连接伺服舵机信号线
int myangle;//定义角度变量
int pulsewidth=1500;//定义脉宽变量          左 1900 中 1500 右 1100
int val;
int p1=20,d1;
int speed1=300;
int mid=84,servopulse,reach;//图像中点
int stop_flag,turn_flag;// 停止标志位，转向标志位
int turn_right_time=120000;//控制舵机向右打转的时间


typedef struct
{
  int data[50][2] = {{0,0}};
  int len = 0;
}List;
List list;

void setup() {
  // put your setup code here, to run once:
  softSerial.begin(9600);
  Serial.begin(9600);
  
//  pinMode(servopin,OUTPUT);//设定舵机接口为输出接口
  servo1.attach(servopin); 
  
  pinMode(4,OUTPUT); 
  pinMode(5,OUTPUT); 
  digitalWrite(4,LOW); 
  TCCR0B = TCCR0B & 0xF8 | 1;
  analogWrite(5, 120);
  
//   analogWrite(5, 200);
//  pinMode(6,OUTPUT); 
  //  pinMode(7,OUTPUT); 
//  digitalWrite(7,LOW); 
//   analogWrite(6, 100);
}

void loop() {
//    digitalWrite(5,HIGH); 
//  delayMicroseconds(3);
//
//  digitalWrite(5,LOW); 
//  delayMicroseconds(7);
////   digitalWrite(4,LOW); 
  
  
  if(softSerial.available())
  {
    getList();
    for (int i=0; i<list.len; i++)
    {
      Serial.print(list.data[i][0]);
      Serial.print('\t');
      Serial.println(list.data[i][1]);

     if(list.data[i][0]==300 and list.data[i][1]==300 )
     {
      stop_flag=1;
      
      
      }
      if(stop_flag==1)
      {
        analogWrite(5, 0);
        }
     if(list.data[i][0]==400 and list.data[i][1]==400 )
     {
      turn_flag=1;
      
      
      }
      if(turn_flag==1)

      {  analogWrite(5, 70);
        servo1.writeMicroseconds(1200);
        delay(120000);
        turn_flag=0;
        analogWrite(5, 120);
        
        }



       
//               servopulse=list.data[i][0]-mid;
//           if(servopulse<-50)
//           servopulse=-50;
//           if(servopulse>50)
//           servopulse=50;
//           
//            servo1.write(90-servopulse);  
  

       reach=list.data[i][0];
       pulsewidth= p1*(reach-mid)+1400;
       if(pulsewidth<1100)
       pulsewidth=1100;
       if(pulsewidth>1900)
       pulsewidth=1900;
       servo1.writeMicroseconds(pulsewidth);
//        Serial.print('\t');
//      Serial.println(pulsewidth);
//       Serial.println("============");
    clearList();
        
        }
     }

     else
     {
      reach=mid;
       pulsewidth= p1*(reach-mid)+1500;
      servo1.writeMicroseconds(pulsewidth);
 //     pulsewidth=1500;
//      digitalWrite(servopin,HIGH);//将舵机接口电平至高
//          delayMicroseconds(pulsewidth);//延时脉宽值的微秒数
//          digitalWrite(servopin,LOW);//将舵机接口电平至低
//          delayMicroseconds(20000-pulsewidth);
      }
 
//          digitalWrite(servopin,HIGH);//将舵机接口电平至高
//          delayMicroseconds(371.2*(pulsewidth));//延时脉宽值的微秒数
//          digitalWrite(servopin,LOW);//将舵机接口电平至低
//          delayMicroseconds(371.2*(20000-pulsewidth));


}


String detectString()
{
  while(softSerial.read() != '{');
  return(softSerial.readStringUntil('}'));
}
void clearList()
{
  memset(list.data, sizeof(list.data),0);
  list.len = 0;
}
void getList()
{
  String s = detectString();
  String numStr = "";
  for(int i = 0; i<s.length(); i++)
  {
    if(s[i]=='('){
      numStr = "";
    }
    else if(s[i] == ','){
      list.data[list.len][0] = numStr.toInt();
      numStr = "";
    }
    else if(s[i]==')'){
      list.data[list.len][1] = numStr.toInt();
      numStr = "";
      list.len++;
    }
    else{
      numStr += s[i];
    }
  }
}
